package pl.edu.agh.pros.node;

import org.ros.exception.RemoteException;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.service.ServiceClient;
import org.ros.node.service.ServiceResponseListener;

public class RosServiceClient extends AbstractNodeMain {

	private static final String SERVICE_NAME = "/test_service"; //$NON-NLS-1$
	private static final String SERVICE_TYPE = "std_srvs/Empty"; //$NON-NLS-1$

	private boolean responseReceived = false;

	@Override
	public GraphName getDefaultNodeName() {
		return new GraphName("pros_core/rosServiceClient"); //$NON-NLS-1$
	}

	@Override
	public void onStart(ConnectedNode node) {
		try {
			System.out.println("Starting a Testing Service Client Node........"); //$NON-NLS-1$

			ServiceClient<std_srvs.Empty.Request, std_srvs.Empty.Response> client = node.newServiceClient(SERVICE_NAME,
					SERVICE_TYPE);
			Thread.sleep(1000);
			client.call(client.newMessage(), new ServiceResponseListener<std_srvs.Empty.Response>() {
				@Override
				public void onSuccess(std_srvs.Empty.Response response) {
					System.out.println("Client: received response"); //$NON-NLS-1$
					responseReceived = true;
				}

				@Override
				public void onFailure(RemoteException e) {
					System.out.println("Client: request failure"); //$NON-NLS-1$
				}
			});

		} catch (Exception e) {
			if (node != null) {
				node.getLog().fatal(e);
			} else {
				e.printStackTrace();
			}
		}

	}

	public boolean isResponseReceived() {
		return responseReceived;
	}

}
